#include  "Timer.h"
#define serialbaud 9600
#define TSampling 60

Timer t;

int flag = 0;
int MotorCtrlTaskID,MeasureID,SerialTaskID;
int MotorSpeed=0;
unsigned long nextSample = 0;

#include <NewPing.h>
NewPing sensor1(5, 6,35);
 
void setup()
{
  Serial.begin(serialbaud);
  pinMode(13, OUTPUT);  
  MotorCtrlTaskID = t.every(400, MotorCtrlTask);
  SerialTaskID = t.every(100, SerialTask);
    
}
 
void MotorCtrlTask()
{
  MotorSpeed = analogRead(0);
  MotorSpeed = map(MotorSpeed, 0, 1023, 0, 100);

  analogWrite(9,MotorSpeed);
  
  //Serial.println();Serial.print(millis());Serial.print(" -- ");Serial.println(MotorSpeed);
   
    
}


void loop()
{
  t.update();
}
 
void SerialTask(){
  if (Serial.available() > 0)
  {
    int Command = Serial.read();
    if (Command == 'a'){
      if(MeasureID==0){
        nextSample=millis()+50;
        MeasureID = t.every(2, DoMeasurement);
      }
    }
    else
    if (Command == '#'){
      if(MeasureID!=0){
        t.stop(MeasureID); 
        MeasureID = 0;
      }    
    }
  }
}
 
void print3digit(int x)
{
  if(x>999){
    x=999;
  }
  if(x<10)
   {
     Serial.print('0');
   }
  if(x<100)
  {
     Serial.print('0');
  }
  Serial.print(x);   
}

void DoMeasurement()
{
  while (millis() < nextSample){}
  //Serial.print(millis());
  //print3digit(millis()%1000); 
  nextSample += 12;  
  int temp = sensor1.ping_mm();  
  nextSample += 12;   
  while (millis() < nextSample){}
  int temp2 = sensor1.ping_mm();  
  nextSample += 12;   
  while (millis() < nextSample){}
  nextSample += 14;  
  int temp3 = sensor1.ping_mm();  
  Serial.write(0xD);
  print3digit(888);
  Serial.write(0x20); 
  print3digit(temp);        
  Serial.write(0x20); 
  print3digit(temp2);        
  Serial.write(0x20); 
  print3digit(temp3);        
}
 

